import roomba
import msvcrt
import time
import math

def RoombaRemote():

    thebot = roomba.RoombaRobot(6)
    
    time.sleep(.2)
    while True:
        #time.sleep(.1)
        (x,y,thr,lftBmp,rgtBmp) = thebot.GetPoseReal()
        
        if(lftBmp or rgtBmp):
            thebot.roomba.drive(300,0)
        
        if msvcrt.kbhit():
            c = msvcrt.getch()
            if c == 'i':
                print 'i'
                thebot.roomba.drive(300,2001)
            if c == 'k':
                print 'k'
                thebot.roomba.drive(-150, 2001)
            if c == 'j':
                print 'j'
                thebot.roomba.drive(300, 300)
            if c == 'l':
                print 'l'
                thebot.roomba.drive(300, -300)
            if c == 'w':
                print 'w'
                thebot.roomba.drive(0,0)
            if c == 'z':
                print 'z'
                thebot.roomba.playNote(60,32)
            if c == 'x':
                print 'z'
                thebot.roomba.playNote(62,32)
            if c == 'c':
                print 'z'
                thebot.roomba.playNote(64,32)
            if c == 'v':
                print 'z'
                thebot.roomba.playNote(65,32)
            if c == 'b':
                print 'z'
                thebot.roomba.playNote(67,32)
            if c == 'n':
                print 'z'
                thebot.roomba.playNote(69,32)
            if c == 'm':
                print 'z'
                thebot.roomba.playNote(71,32)
            if c == ',':
                print 'z'
                thebot.roomba.playNote(72,32)
            if c == 'a':
                print 'z'
                thebot.roomba.playNote(55,32)
            if c == 's':
                print 'z'
                thebot.roomba.playNote(57,32)
            if c == 'd':
                print 'z'
                thebot.roomba.playNote(59,32)
            if c == 'f':
                print 'z'
                thebot.roomba.playNote(60,32)

            if c == 'q':
                break

'''
def findGoal(x,y):

    thebot = roomba.RoombaRobot(6)

    thebot.roomba.resetPose()
    thebot.roomba.reset
    goal = (x,y)

    while True:
        [x,y,thd], bump = self.getData()
        thr = math.radians(thd)


        dx = goal[0]-x
        dy = goal[1]-y
        distanceToGoal = math.sqrt(dx*dx+dy*dy)
        #mytheta = thr - math.pi
        try:
            absoluteAngle = math.atan2(dy,dx)
            angleToGoal =  absoluteAngle - thr 
        except:
            angleToGoal = 0
        if angleToGoal < -1*math.pi:
            angleToGoal += 2*math.pi
        if angleToGoal > math.pi:
            angleToGoal -= 2*math.pi

        # This stops if a bump is encountered - a pretty basic plan
        if bump[0] == True or bump[1] == True:
            print 'BUMP!',
            print '  [Left bump sensor:',  bump[0], ']  ',
            print '  [Right bump sensor:', bump[1], ']  '
            robotTask == BUMP


        if robotTask == TRACKGOAL:
            if angleToGoal > 0:
                self.roomba.drive(200, 300)
            else:
                self.roomba.drive(200, -300)

        if robotTask == BUMP                
            if bump[0]:
                self.roomba.drive(-200,0)
            elif bump[1]:
                self.roomba.drive(200,0)
            else:
                robotTask == TRACKGOAL

'''
def reset():
    thebot = roomba.RoombaRobot(6)
    time.sleep(.2)
    thebot.roomba.ser.write( chr(128) )
    thebot.roomba.ser.write( chr(131) )
    thebot.roomba.ser.write( chr(128) )
    thebot.roomba.ser.write( chr(131) )

def rotTest():
    thebot = roomba.RoombaRobot(6)
    time.sleep(.2)
    thebot.SetPose(0,0,0)
    thebot.roomba.drive(75,0)
    time.sleep(4*2.79)
    thebot.roomba.drive(0,0)

def square():
    thebot = roomba.RoombaRobot(6)
    #thebot.roomba.start()
    time.sleep(.2)
    #thebot.roomba.ser.write( chr(128) )
    #thebot.roomba.ser.write( chr(132) )
    currAng = 0
    state = 0
    forwardVel = 150
    turnVel = 75
    thebot.SetPose(0,0,0)
    thebot.roomba.drive(forwardVel,2001)
    while True:
        (x,y,thr,lb,rb) = thebot.GetPoseReal()
        if(x > 66.68 and state == 0):
            state += 1
            thebot.roomba.drive(turnVel,0)
        if(thr > math.pi/2.0 and state ==1):
            state += 1
            thebot.roomba.drive(forwardVel, 2001)
        if(y > 66.68 and state == 2):
            state += 1
            thebot.roomba.drive(turnVel,0)
        if(thr > math.pi and state == 3):
            state += 1
            thebot.roomba.drive(forwardVel,2001)
        if(x < 0 and state == 4):
            state += 1
            thebot.roomba.drive(turnVel,0)
        if(thr > 3.0/2.0*math.pi and state == 5):
            state += 1
            thebot.roomba.drive(forwardVel,2001)
        if(y < 0 and state == 6):
            thebot.roomba.drive(turnVel,0)
            state += 1
        if(thr > 2*math.pi and state == 7):
            thebot.roomba.drive(0,0)
            state = 0
            
            
         
    
    
        #(x,y,thr,lb,rb) = thebot.GetPoseReal()
        print x,y,thr,lb,rb
        #if(x > 66.68):
        #    thebot.roomba.drive(0,0)
        if msvcrt.kbhit():
            c = msvcrt.getch()
            if c == 'q':
                break
            if c == 'w':
                thebot.roomba.drive(0,0)
            if c == 'y':
                thebot.roomba(-150,0)
            if c == 'm':
                thebot.roomba.drive(150,2001)
                state = 0
                thebot.SetPose(0,0,0)

if __name__ == '__main__':      # are we at the command line
    reset()
    #square()
    RoombaRemote()
